/***************************************************************************
*   Copyright (C) 2010-2011 by swkyer <swkyer@gmail.com>                  *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License as published by  *
*   the Free Software Foundation; either version 2 of the License, or     *
*   (at your option) any later version.                                   *
*                                                                         *
*   This program is distributed in the hope that it will be useful,       *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU General Public License for more details.                          *
*                                                                         *
*   You should have received a copy of the GNU General Public License     *
*   along with this program; if not, write to the                         *
*   Free Software Foundation, Inc.,                                       *
*   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
***************************************************************************/
#include "stdafx.h"
#include "mips32opc.h"
#include "JlinkMIPS.h"
#include "Execution.h"
#include "Memory.h"



JLINKMIPS_API int mips32_read_byte(mips32_t *pmips32, ubase_t addr, uint8_t *pbyte, int length)
{
    ubase_t adr1;
    uint8_t *bytev1, *bytev2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pbyte == NULL || length <= 0)
        return -1;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_readbyte);
        if (retv < 0)
            return retv;
        *pbyte = (uint8_t)(pcontext->reg_arg1 & 0xff);

        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        bytev1 = pbyte;
        bytev2 = (uint8_t *)pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE)
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readbytes);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE;
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;

            memcpy(bytev1, bytev2, MIPS32_VIRTUAL_RAM_SIZE);
            bytev1 += MIPS32_VIRTUAL_RAM_SIZE;
        }

        if (len1 > 0)
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + len1;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readbytes);
            if (retv < 0)
                return retv;
            memcpy(bytev1, bytev2, len1);
            adr1 += len1;
        }

        return (adr1 - addr);
    }
}

JLINKMIPS_API int mips32_write_byte(mips32_t *pmips32, ubase_t addr, uint8_t *pbyte, int length)
{
    ubase_t adr1;
    uint8_t *bytev1, *bytev2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pbyte == NULL || length <= 0)
        return -1;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        pcontext->reg_arg1 = *pbyte;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_writebyte);
        if (retv < 0)
            return retv;
        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        bytev1 = pbyte;
        bytev2 = (uint8_t *)pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE)
        {
            memcpy(bytev2, bytev1, MIPS32_VIRTUAL_RAM_SIZE);
            bytev1 += MIPS32_VIRTUAL_RAM_SIZE;

            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writebytes);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE;
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;
        }

        if (len1 > 0)
        {
            memcpy(bytev2, bytev1, len1);

            pcontext->reg_arg0 = addr;
            pcontext->reg_arg1 = addr + len1;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writebytes);
            if (retv < 0)
                return retv;
            adr1 += len1;
        }

        return (adr1 - addr);
    }
}

JLINKMIPS_API int mips32_read_halfword(mips32_t *pmips32, ubase_t addr, uint16_t *pshort, int length)
{
    ubase_t adr1;
    uint16_t *shortv1, *shortv2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pshort == NULL || length <= 0)
        return -1;
    if (addr & (sizeof(uint16_t)-1))
        return -2;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_readhalf);
        if (retv < 0)
            return retv;
        *pshort = (uint16_t)(pcontext->reg_arg1 & 0xffff);

        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        shortv1 = pshort;
        shortv2 = (uint16_t *)pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint16_t))
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readhalfs);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint16_t);
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;

            memcpy(shortv1, shortv2, MIPS32_VIRTUAL_RAM_SIZE);
            shortv1 += MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint16_t);
        }

        if (len1 > 0)
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + len1*sizeof(uint16_t);
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readhalfs);
            if (retv < 0)
                return retv;
            memcpy(shortv1, shortv2, len1*sizeof(uint16_t));
            adr1 += len1*sizeof(uint16_t);
        }

        return ((adr1 - addr)/sizeof(uint16_t));
    }
}

JLINKMIPS_API int mips32_write_halfword(mips32_t *pmips32, ubase_t addr, uint16_t *pshort, int length)
{
    ubase_t adr1;
    uint16_t *shortv1, *shortv2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pshort == NULL || length <= 0)
        return -1;
    if (addr & (sizeof(uint16_t)-1))
        return -2;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        pcontext->reg_arg1 = *pshort;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_writehalf);
        if (retv < 0)
            return retv;
        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        shortv1 = pshort;
        shortv2 = (uint16_t *)pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint16_t))
        {
            memcpy(shortv1, shortv2, MIPS32_VIRTUAL_RAM_SIZE);
            shortv1 += MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint16_t);

            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writehalfs);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint16_t);
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;
        }

        if (len1 > 0)
        {
            memcpy(shortv2, shortv1, len1*sizeof(uint16_t));

            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + len1 * sizeof(uint16_t);
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writehalfs);
            if (retv < 0)
                return retv;
            adr1 += len1 * sizeof(uint16_t);
        }

        return ((adr1 - addr)/sizeof(uint16_t));
    }
}

JLINKMIPS_API int mips32_read_word(mips32_t *pmips32, ubase_t addr, uint32_t *intv, int length)
{
    ubase_t adr1;
    uint32_t *intv1, *intv2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || intv == NULL || length <= 0)
        return -1;
    if (addr & (sizeof(uint32_t)-1))
        return -2;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_readword);
        if (retv < 0)
            return retv;
        *intv = (uint32_t)pcontext->reg_arg1;

        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        intv1 = intv;
        intv2 = pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint32_t))
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readwords);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;

            memcpy(intv1, intv2, MIPS32_VIRTUAL_RAM_SIZE);
            intv1 += MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);
        }

        if (len1 > 0)
        {
            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + len1*sizeof(uint32_t);
            retv = mips32_exec_microcode(pcontext, mips32_microcode_readwords);
            if (retv < 0)
                return retv;
            memcpy(intv1, intv2, len1*sizeof(uint32_t));
            adr1 += len1*sizeof(uint32_t);
        }

        return ((adr1 - addr)/sizeof(uint32_t));
    }
}

JLINKMIPS_API int mips32_write_word(mips32_t *pmips32, ubase_t addr, uint32_t *intv, int length)
{
    ubase_t adr1;
    uint32_t *intv1, *intv2;
    int retv, len1;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || intv == NULL || length <= 0)
        return -1;
    if (addr & (sizeof(uint32_t)-1))
        return -2;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    if (length == 1)
    {
        pcontext->reg_arg0 = addr;
        pcontext->reg_arg1 = *intv;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_writeword);
        if (retv < 0)
            return retv;
        return 1;
    }
    else
    {
        len1 = length;
        adr1 = addr;
        intv1 = intv;
        intv2 = pcontext->vram;

        while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint32_t))
        {
            memcpy(intv2, intv1, MIPS32_VIRTUAL_RAM_SIZE);
            intv1 += MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);

            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + MIPS32_VIRTUAL_RAM_SIZE;
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writeword);
            if (retv < 0)
                return retv;
            len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);
            adr1 += MIPS32_VIRTUAL_RAM_SIZE;
        }

        if (len1 > 0)
        {
            memcpy(intv2, intv1, len1*sizeof(uint32_t));

            pcontext->reg_arg0 = adr1;
            pcontext->reg_arg1 = adr1 + len1 * sizeof(uint32_t);
            retv = mips32_exec_microcode(pcontext, mips32_microcode_writeword);
            if (retv < 0)
                return retv;
            adr1 += len1 * sizeof(uint32_t);
        }

        return ((adr1 - addr)/sizeof(uint32_t));
    }
}

JLINKMIPS_API int mips32_read_memory(mips32_t *pmips32, ubase_t addr, uint8_t *pbyte, int length)
{
    int retv, len1, len2;
    uint8_t *bytev1;
    ubase_t adr;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pbyte == NULL || length <= 0)
        return -1;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    // d-word aligned
    retv = addr % sizeof(uint32_t);
    adr = addr - retv;
    len1 = length + retv;
    len2 = len1 % sizeof(uint32_t);
    if (len2 != 0)
        len2 = sizeof(uint32_t) - len2;
    len1 += len2;
    bytev1 = pbyte;

    // read words
    len1 = len1 / sizeof(uint32_t);
    while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint32_t))
    {
        pcontext->reg_arg0 = adr;
        pcontext->reg_arg1 = adr + MIPS32_VIRTUAL_RAM_SIZE;
        retv = mips32_exec_microcode(pcontext, mips32_microcode_readwords);
        if (retv < 0)
            return retv;
        memcpy(bytev1, pcontext->vram, MIPS32_VIRTUAL_RAM_SIZE);
        adr += MIPS32_VIRTUAL_RAM_SIZE;
        bytev1 += MIPS32_VIRTUAL_RAM_SIZE;
        len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);
    }
    if (len1 > 0)
    {
        pcontext->reg_arg0 = adr;
        pcontext->reg_arg1 = adr + len1*sizeof(uint32_t);
        retv = mips32_exec_microcode(pcontext, mips32_microcode_readwords);
        if (retv < 0)
            return retv;
        memcpy(bytev1, pcontext->vram, len1*sizeof(uint32_t));
        adr += len1*sizeof(uint32_t);
        bytev1 += len1*sizeof(uint32_t);
    }

    return (adr - addr - len2);
}

JLINKMIPS_API int mips32_write_memory(mips32_t *pmips32, ubase_t addr, uint8_t *pbyte, int length)
{
    int retv, len1;
    uint8_t *bytev1, byteval;
    ubase_t adr;
    mips32_exec_context_t *pcontext = &pmips32->execontext;

    if (pmips32 == NULL || pmips32->ejtag == NULL 
            || pbyte == NULL || length <= 0)
        return -1;

    mips32_get_state(pmips32);
    if (pmips32->target_state != CORE_STATE_HALTED)
    {
        // target is running
        return -3;
    }

    len1 = addr % sizeof(unsigned int);
    if (len1 != 0)
    {
        // read a word
        pcontext->reg_arg0 = addr - len1;
        mips32_exec_microcode(pcontext, mips32_microcode_readword);

        bytev1 = pbyte;
        for (retv = 0; retv < len1; retv++)
        {
            byteval = retv*8;
            byteval = (uint8_t)((pcontext->reg_arg1 & (0xFF << byteval)) >> byteval);
            *bytev1++ = byteval;
        }

        length += len1;
        addr -= len1;
    }

    len1 = (addr + length) % sizeof(uint32_t);
    if (len1 != 0)
    {
        // read a word
        pmips32->execontext.reg_arg0 = addr + length - len1;
        mips32_exec_microcode(pcontext, mips32_microcode_readword);

        bytev1 = pbyte + length;
        for (retv = len1; retv < sizeof(uint32_t); retv++)
        {
            byteval = retv*8;
            byteval = (uint8_t)((pcontext->reg_arg1 & (0xFF << byteval)) >> byteval);
            *bytev1++ = byteval;
        }

        length += sizeof(uint32_t) - len1;
    }

    // write words
    adr = addr;
    bytev1 = pbyte;
    len1 = length / sizeof(uint32_t);
    while (len1 > MIPS32_VIRTUAL_RAM_SIZE/sizeof(uint32_t))
    {
        pcontext->reg_arg0 = adr;
        pcontext->reg_arg1 = adr + MIPS32_VIRTUAL_RAM_SIZE;
        memcpy(pcontext->vram, bytev1, MIPS32_VIRTUAL_RAM_SIZE);
        retv = mips32_exec_microcode(pcontext, mips32_microcode_writewords);
        if (retv < 0)
            return retv;
        adr += MIPS32_VIRTUAL_RAM_SIZE;
        bytev1 += MIPS32_VIRTUAL_RAM_SIZE;
        len1 -= MIPS32_VIRTUAL_RAM_SIZE / sizeof(uint32_t);
    }
    if (len1 > 0)
    {
        memcpy(pcontext->vram, bytev1, len1*sizeof(uint32_t));
        pcontext->reg_arg0 = adr;
        pcontext->reg_arg1 = adr + len1*sizeof(uint32_t);
        retv = mips32_exec_microcode(pcontext, mips32_microcode_writewords);
        if (retv < 0)
            return retv;
        adr += len1*sizeof(uint32_t);
        bytev1 += len1*sizeof(uint32_t);
    }

    return (adr - addr);
}

